%% Entry Guidance With Terminal Time Control Based on Quasi-Equilibrium Glide Condition
clc
clear
close all
global R0 g0 rot Vs ts m S d2r v1 v2 alphaMax alphaLD alphaMin kh1 kh2 Hs...
       delta_azi0 delta_azif absBankMax Stogo0
d2r = pi/180;
m = 907;
S = 0.48;
R0 = 6378135;
g0 = 9.81;
rot = 7.292115E-5;
Vs = sqrt(g0*R0);
ts = sqrt(R0/g0);
v1 = 4700;
v2 = 3100;
alphaMax = 20*pi/180;
alphaLD = 12*pi/180;
alphaMin = 5*pi/180;
absBankMax = 60*pi/180;
kh1 = 0.05;
kh2 = 0.01;
Hs = 7254.2;
h0 = 80e3;
lon0 = 0;
lat0 = 0;
V0 = 6500;
fpa0 = -0.4*d2r;
azi0 = 0;
lonf = 0;
latf = 85*d2r;
hf = 30e3;
radf = hf + R0;
Vf = 2e3;
tf = 2000;
ef = R0/radf - Vf^2/(2*g0*R0);
Stogo0 = acos(sin(lat0)*sin(latf) + cos(lat0)*cos(latf)*cos(lonf-lon0));
delta_azi0 = 10*d2r;
delta_azif = 10*d2r;

dt = 0.1;
p = 1;
state(p,:) = [h0+R0, lon0, lat0, V0, fpa0, azi0];
statef = [radf, lonf, latf, Vf, nan, nan];
time(p,1)=0;
ctrl(p,:)= [20*pi/180, 0, 1];
stage = [0 0 0];
% descend phase
while 1
    state_temp = RK4(@ReentryDynamic, state(p,:), ctrl(p,:), dt);
    ctrl(p+1,:) = GuidanceDes();
    state(p+1,:) = state_temp;
    time(p+1,1) = time(p,1) + dt;
    p = p+1;
    if state_temp(5) >= 0
        stage(1,1) = time(p, 1);
        break;
    end
end
% hight ascend phase
while 1
    state_temp = RK4(@ReentryDynamic, state(p,:), ctrl(p,:), dt);
    ctrl_temp = GuidanceStage2(state(p,:), ctrl(p,:));
    alpha = limitRate(ctrl_temp(1), ctrl(p,1), dt, 5*pi/180);
    bank = limitRate(ctrl_temp(2), ctrl(p,2), dt, 30*pi/180);
    ctrl(p+1,:) = [alpha bank ctrl_temp(3)];
    state(p+1,:) = state_temp;
    time(p+1,1) = time(p,1) + dt;
    p = p+1;
    if state(p,1) < state(p-1,1)
        stage(1,2) = time(p, 1);
        break;
    end
end
% gliding phase
while 1
    state_temp = RK4(@ReentryDynamic, state(p,:), ctrl(p,:), dt);
    ctrl_temp = GuidanceGli(time(p,1), tf, state(p,:), statef, ctrl(p,:));
    alpha = limitRate(ctrl_temp(1), ctrl(p,1), dt, 5*pi/180);
    bank = limitRate(ctrl_temp(2), ctrl(p,2), dt, 30*pi/180);
    ctrl(p+1,:) = [alpha bank ctrl_temp(3)];
    state(p+1,:) = state_temp;
    time(p+1,1) = time(p,1) + dt;
    p = p+1;
%     h = state_temp(1) - R0;
%     if h <= hf
%         stage(1,3) = time(p, 1);
%         break;
%     end
    r = state_temp(1)/R0;
    V = state_temp(4)/Vs;
    e = 1/r - V^2/2;
    if e >= ef
        break;
    end
end

%% plot
alt = state(:, 1) - R0;
lon = state(:, 2)*180/pi;
lat = state(:, 3)*180/pi;
V = state(:, 4);
fpa = state(:, 5)*180/pi;
azi = state(:, 6);
alpha = ctrl(:, 1)*180/pi;
absbank = ctrl(:, 2)*180/pi;
signOfBank = ctrl(:, 3);
bank = signOfBank.*absbank;

figure
plot3(lon, lat, alt, 'linewidth', 2);
xlabel('Longitude');ylabel('Latitude');zlabel('Altitude (km)');
grid on

figure
pp = plot(time, alt, 'linewidth', 1.5);
xlabel('Time (s)'); ylabel('Altitude (km)');

figure
plot(lon, lat, 'linewidth', 1.5);
xlabel('Longitude (deg)'); ylabel('Latitude (deg)');

figure
plot(time, V, 'linewidth', 1.5);
xlabel('Time (s)'); ylabel('Speed (km/s)');

figure
plot(time, fpa, 'linewidth', 1.5);
xlabel('Time (s)'); ylabel('Flight Path Angle (deg)');

figure
plot(time, azi, 'linewidth', 1.5);
xlabel('Time (s)'); ylabel('Azimuth Angle (deg)');

figure
plot(time, alpha, 'linewidth', 1.5);
xlabel('Time (s)'); ylabel('Angle of Attack (deg)');

figure
plot(time, bank, 'linewidth', 1.5);
xlabel('Time (s)'); ylabel('Bank Angle (deg)');

figure
plot(time, signOfBank, 'o');
xlabel('Time (s)'); ylabel('Sign Of Bank Angle (deg)');